#ifndef BTL_VIEWGEOMETRY_DISAMBIGUATEESSENTIAL
#define BTL_VIEWGEOMETRY_DISAMBIGUATEESSENTIAL

#include "EssentialMatrix.hpp"
#include "PointTriangulation.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <iostream>

namespace btl
{
namespace viewgeometry
{

class DisambiguateEssential
{
   public:
      DisambiguateEssential();
      DisambiguateEssential(
         const Eigen::Matrix3d& Rx, const Eigen::Matrix3d& Ry, const Eigen::Vector3d& t,
         const Eigen::Vector3d& x1, const Eigen::Vector3d& x2);
      void compute(
         const Eigen::Matrix3d& Rx, const Eigen::Matrix3d& Ry, const Eigen::Vector3d& t,
         const Eigen::Vector3d& x1, const Eigen::Vector3d& x2);

      const Eigen::Isometry3d& pose() const;
      const Eigen::Vector4d& x() const;
      const Eigen::Matrix3d& E() const;

   private:
      Eigen::Isometry3d _pose;
      Eigen::Matrix3d _E;
      Eigen::Vector4d _x;
};

} // namespace viewgeometry
} // namespace btl

namespace btl
{

using viewgeometry::DisambiguateEssential;

} // namespace btl

// ====================================================================
// === Implementation

namespace btl
{
namespace viewgeometry
{

inline DisambiguateEssential::DisambiguateEssential()
{
}

inline DisambiguateEssential::DisambiguateEssential(
   const Eigen::Matrix3d& Rx, const Eigen::Matrix3d& Ry, const Eigen::Vector3d& t,
   const Eigen::Vector3d& x1, const Eigen::Vector3d& x2)
{
   compute(Rx, Ry, t, x1, x2);
}

inline void DisambiguateEssential::compute(
   const Eigen::Matrix3d& Rx, const Eigen::Matrix3d& Ry, const Eigen::Vector3d& t,
   const Eigen::Vector3d& x1, const Eigen::Vector3d& x2)
{
   Eigen::Isometry3d P;
   P.matrix().topLeftCorner<3,3>() = Rx;
   P.matrix().topRightCorner<3,1>() = t;
   P.matrix().row(3) = Eigen::RowVector4d(0.0, 0.0, 0.0, 1.0);

   _E = BuildEssentialMatrix(P).matrix();

   Eigen::Vector4d pt1 = triangulate(P, _E, x1, x2);
   Eigen::Vector4d pt2 = P.matrix() * pt1;

   if (pt1.z() < 0.0 || pt2.z() < 0.0)
   {
      if (pt1.z() < 0.0 && pt2.z() < 0.0)
      {
         P.matrix().topRightCorner<3,1>() *= -1.0;
      }
      else
      {
         P.matrix().topLeftCorner<3,3>() = Ry;
         pt1 = triangulate(P, _E, x1, x2);
         pt2 = P.matrix() * pt1;

         if (pt1.z() < 0.0 || pt2.z() < 0.0)
         {
            P.matrix().topRightCorner<3,1>() *= -1.0;
            pt1 = triangulate(P, _E, x1, x2);
         }
      }
   }

   _pose = P;
   _x = pt1;
}

inline const Eigen::Isometry3d& DisambiguateEssential::pose() const
{
   return _pose;
}

inline const Eigen::Vector4d& DisambiguateEssential::x() const
{
   return _x;
}

inline const Eigen::Matrix3d& DisambiguateEssential::E() const
{
   return _E;
}

} // namespace viewgeometry
} // namespace btl

#endif // BTL_VIEWGEOMETRY_DISAMBIGUATEESSENTIAL
